#include "WPILib.h"
#include "LimitedVictor.h"
#include <cmath>

LimitedVictor::LimitedVictor(int channel, float slopeLimit, float minimumOutput, float maximumOutput) : Victor(channel)
{
	this->slopeLimit = slopeLimit;
	
	min = minimumOutput;
	max = maximumOutput;
	
	limitOutput = true;
	limitSlope = true;
}

void LimitedVictor::LimitOutput(bool value)
{
	limitOutput = value;
}

void LimitedVictor::LimitSlope(bool value)
{
	limitSlope = value;
}

void LimitedVictor::Set(float value, UINT8 syncGroup)
{
	if (limitSlope && abs(Get() - value) > slopeLimit)
	{
		value = Get() + ((value > Get()) ? slopeLimit : -slopeLimit);
	}
	
	if (limitOutput)
	{
		if (value < min) value = min;
		else if (value > max) value = max;
	}
	
	Victor::Set(value);
}
